﻿#region File Description
//-----------------------------------------------------------------------------
// ModelViewerControl.cs
//
// Microsoft XNA Community Game Platform
// Copyright (C) Microsoft Corporation. All rights reserved.
//-----------------------------------------------------------------------------
#endregion

#region Using Statements
using System;
using System.Diagnostics;
using System.Windows.Forms;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using System.Collections.Generic;
#endregion

namespace WinFormsContentLoading
{
    /// <summary>
    /// Example control inherits from GraphicsDeviceControl, and displays
    /// a spinning 3D model. The main form class is responsible for loading
    /// the model: this control just displays it.
    /// </summary>
    class ModelViewerControl : GraphicsDeviceControl
    {
        /// <summary>
        /// Gets or sets the current model.
        /// </summary>
        /// 

        //手臂模型
        Model model;
        public Model Model
        {
            get { return model; }

            set
            {
                model = value;

                if (model != null)
                {
                    BoneProcess();
                    MeasureModel();
                }
            }
        }

        //房間場景模型
        Model room;
        public Model Room
        {
            get { return room; }

            set
            {
                room = value;
                if (room != null)
                {
                    BoneProcess_room();
                }
            }
        }

        float rotation;
        public float Rotation
        {
            get { return rotation; }

            set
            {
                rotation = value;
            }
        }


        // Cache information about the model size and position.
        //存手臂的boneTransforms
        Matrix[] boneTransforms;
        //存房間場景的boneTransforms
        Matrix[] boneTransforms_room;
        Vector3 modelCenter;
        float modelRadius;
        double CameraRotateValue;
        double CameraTiltValue;
        protected Ray mouseRay;
        Matrix world;
        Matrix view;
        Matrix projection;
        // 計時器
        // Timer controls the rotation speed.
        // Stopwatch timer;

        #region Fields

        // The XNA framework Model object that we are going to display.
        // Model robotModel;

        // Shortcut references to the bones that we are going to animate.
        // We could just look these up inside the Draw method, but it is more
        // efficient to do the lookups while loading and cache the results.
        ModelBone J1_Bone;
        ModelBone J2_Bone;
        ModelBone J3_Bone;
        ModelBone J4_Bone;
        ModelBone J5_Bone;
        ModelBone J6_Bone;
        ModelBone CSF14_Bone;

        // Store the original transform matrix for each animating bone.
        Matrix J1_Transform; //底座的
        Matrix J2_Transform;
        Matrix J3_Transform;
        Matrix J4_Transform;
        Matrix J5_Transform;
        Matrix J6_Transform;
        Matrix CSF14_Transform; //末端的

        // Current animation positions.
        float J1RotationValue = 0;
        float J2RotationValue = 0;
        float J3RotationValue = 0;
        float J4RotationValue = 0;
        float J5RotationValue = 0;
        float J6RotationValue = 0;

        /*畫座標軸*/
        VertexPositionColor[] worldAxes; //世界座標
        float axisLength = 1000f;        //世界座標軸的長度
        VertexPositionColor[] localAxes; //Local座標
        float localAxisLength = 80f;     //Local座標軸的長度

        /*軌跡記錄*/
        Stopwatch recordTimer;  //計時器
        bool isRecordPathValue; //是否要記錄軌跡
        double threshold = 200; //設定多少毫秒記錄一次
        List<VertexPositionColor[]> path = new List<VertexPositionColor[]>(); //紀錄軌跡的List

        /*繪出手臂各部分時的修正舉陣*/
        Matrix[] boneModify;

        /*繪出房間場景時的修正舉陣*/
        Matrix roomModify;

        /*目前碰撞狀態*/
        //bool collideResults;
        //BoundingBox primaryBD_Box;
        //BoundingBox[] BD_Boxes = new BoundingBox[6];

        /*要畫控制圈的部位,索引值*/
        int Circle_BoneIndexValue = 0;

        /*要畫local座標軸的部位,索引值 (預設值為末端 = 7)*/
        int LocalAxes_BoneIndexValue = 7;

        /*繪圖模式切換(solid和wire)*/
        bool isWireModeValue = false; //是否要顯示WireFrame
        RasterizerState WireState = new RasterizerState();   //
        RasterizerState SolidState = new RasterizerState();  //

        /*是否為碰撞狀態*/
        bool isCollisionValue = false;

        BoundingBox[] BBox;
        BoundingFrustum frustum;
        Vector2 circleCenter;
        int[] BoneCount;
        #endregion

        #region Properties


        public float J1Rotation
        {
            get { return J1RotationValue; }
            set { J1RotationValue = value; }
        }

        public float J2Rotation
        {
            get { return J2RotationValue; }
            set { J2RotationValue = value; }
        }

        public float J3Rotation
        {
            get { return J3RotationValue; }
            set { J3RotationValue = value; }
        }

        public float J4Rotation
        {
            get { return J4RotationValue; }
            set { J4RotationValue = value; }
        }

        public float J5Rotation
        {
            get { return J5RotationValue; }
            set { J5RotationValue = value; }
        }

        public float J6Rotation
        {
            get { return J6RotationValue; }
            set { J6RotationValue = value; }
        }

        public double CameraRotate
        {
            get { return CameraRotateValue; }
            set { CameraRotateValue = value; }
        }

        public double CameraTilt
        {
            get { return CameraTiltValue; }
            set
            {
                //if (value > Math.PI / 4)
                //    value = Math.PI / 4;
                //因為要上視圖,取消角度上限
                if (value < 0)
                    value = 0;
                //2013.05.05修改: 攝影機不低於地板
                CameraTiltValue = value;
            }
        }



        public Ray MouseRay
        {
            get { return mouseRay; }
        }

        public Vector2 CircleCenter
        {
            get { return circleCenter; }
        }

        public int Circle_BoneIndex
        {
            get { return Circle_BoneIndexValue; }
            set { Circle_BoneIndexValue = value; }
        }

        public int DrawLocalAxes_BoneIndex
        {
            get { return LocalAxes_BoneIndexValue; }
            set { LocalAxes_BoneIndexValue = value; }
        }

        public bool isWireMode
        {
            get { return isWireModeValue; }
            set { isWireModeValue = value; }
        }

        public bool isCollision
        {
            get { return isCollisionValue; }
            set { isCollisionValue = value; }
        }

        public bool isRecordPath
        {
            get { return isRecordPathValue; }
            set { isRecordPathValue = value; }
        }

        #endregion


        /// <summary>
        /// Initializes the control.
        /// </summary>
        protected override void Initialize()
        {
            // Start the animation timer.
            // timer = Stopwatch.StartNew();
            // Hook the idle event to constantly redraw our animation.
            Application.Idle += delegate { Invalidate(); };

            /*世界原點座標軸*/
            worldAxes = new VertexPositionColor[6];
            //x軸 - 紅色
            worldAxes[0] = new VertexPositionColor(Vector3.Zero, Color.Red);
            worldAxes[1] = new VertexPositionColor(new Vector3(axisLength, 0, 0), Color.Red);
            //y軸 - 綠色
            worldAxes[2] = new VertexPositionColor(Vector3.Zero, Color.Green);
            worldAxes[3] = new VertexPositionColor(new Vector3(0, axisLength, 0), Color.Green);
            //z軸 - 藍色
            worldAxes[4] = new VertexPositionColor(Vector3.Zero, Color.Blue);
            worldAxes[5] = new VertexPositionColor(new Vector3(0, 0, axisLength), Color.Blue);

            /*local座標軸*/
            localAxes = new VertexPositionColor[6];

            /*軌跡記錄用的記時器*/
            recordTimer = Stopwatch.StartNew();

            /*手臂修正矩陣*/
            boneModify = new Matrix[8];
            boneModify[0] = Matrix.Identity;                               //J1_Structure-DC_Shell 
            boneModify[1] = Matrix.CreateRotationX((float)(-Math.PI / 2)); //J2_Structure-DC_Shell
            boneModify[2] = Matrix.Identity;                               //J3_Structure-DC_Shell
            boneModify[3] = Matrix.CreateRotationY((float)(-Math.PI / 2)); //J4_Structure-DC_Shell
            boneModify[4] = Matrix.CreateRotationY((float)(-Math.PI / 2)); //J5_Structure-DC_Shell
            boneModify[5] = Matrix.Identity;                               //J6_DC_Shell
            boneModify[6] = Matrix.Identity;                               //未使用到
            boneModify[7] = Matrix.CreateRotationY((float)(-Math.PI / 2)) * Matrix.CreateRotationX((float)(- 150*Math.PI/180 )); //CSF14_8200(CSG/CSF_14)-DC_Shell
            //末端修正 2012.05.03

            /*房間場景修正矩陣*/
            roomModify = Matrix.CreateScale(100) * Matrix.CreateTranslation(-1000, 1800, 0);

            BBox = new BoundingBox[6];
            BoneCount = new int[6];
            frustum = new BoundingFrustum(Matrix.Identity);
            DebugShapeRenderer.Initialize(GraphicsDevice);

            /*兩種模式狀態*/
            WireState.FillMode = FillMode.WireFrame;  //WireFrame模式
            SolidState.FillMode = FillMode.Solid;     //Solid模式(正常模式)
        }

        /*
        private BoundingBox BuildBoundingBox(ModelMesh mesh, Matrix meshTransform)
        {
            // Create initial variables to hold min and max xyz values for the mesh
            Vector3 meshMax = new Vector3(float.MinValue);
            Vector3 meshMin = new Vector3(float.MaxValue);

            foreach (ModelMeshPart part in mesh.MeshParts)
            {
                // The stride is how big, in bytes, one vertex is in the vertex buffer
                // We have to use this as we do not know the make up of the vertex
                int stride = part.VertexBuffer.VertexDeclaration.VertexStride;

                VertexPositionNormalTexture[] vertexData = new VertexPositionNormalTexture[part.NumVertices];
                part.VertexBuffer.GetData(part.VertexOffset * stride, vertexData, 0, part.NumVertices, stride);

                // Find minimum and maximum xyz values for this mesh part
                Vector3 vertPosition = new Vector3();

                for (int i = 0; i < vertexData.Length; i++)
                {
                    vertPosition = vertexData[i].Position;

                    // update our values from this vertex
                    meshMin = Vector3.Min(meshMin, vertPosition);
                    meshMax = Vector3.Max(meshMax, vertPosition);
                }
            }

            // transform by mesh bone matrix
            meshMin = Vector3.Transform(meshMin, meshTransform);
            meshMax = Vector3.Transform(meshMax, meshTransform);

            // Create the bounding box
            BoundingBox box = new BoundingBox(meshMin, meshMax);
            return box;
        }
        */

        public void ResetCamera()
        {
            CameraRotateValue = Math.PI / 4;
            CameraTiltValue = Math.PI / 4;
        }

        public void Camera_FrontView()
        {
            CameraRotateValue = Math.PI;
            CameraTiltValue = 0;
        }

        public void Camera_RightView()
        {
            CameraRotateValue = Math.PI / 2;
            CameraTiltValue = 0;
        }

        public void Camera_TopView()
        {
            CameraRotateValue = 0;
            CameraTiltValue = Math.PI / 2;
        }

        public Ray GetMouseRay(Vector2 mousePosition, Viewport viewport)
        {
            Vector3 near = new Vector3(mousePosition, 0);
            Vector3 far = new Vector3(mousePosition, 1);

            near = viewport.Unproject(near, projection, view, Matrix.Identity);
            far = viewport.Unproject(far, projection, view, Matrix.Identity);

            return new Ray(near, Vector3.Normalize(far - near));
        }

        public virtual void UpdateMouseRay(int X, int Y )
        {
            Vector2 mousePos = new Vector2( X, Y);
            mouseRay = this.GetMouseRay(mousePos, GraphicsDevice.Viewport);
        }

        public void BoneProcess()
        {
            // Load the model from the ContentManager.
            // robotModel = content.Load<Model>("robot");

            // Look up shortcut references to the bones we are going to animate.
            // 重新命名 by 晉瑋 2012.12.17
            J1_Bone = model.Bones["J1_Structure-DC_Shell"];
            J2_Bone = model.Bones["J2_Structure-DC_Shell"];
            J3_Bone = model.Bones["J3_Structure-DC_Shell"];
            J4_Bone = model.Bones["J4_Structure-DC_Shell"];
            J5_Bone = model.Bones["J5_Structure-DC_Shell"];
            J6_Bone = model.Bones["J6-DC_Shell"];
            CSF14_Bone = model.Bones["CSF14_8200(CSG/CSF_14)-DC_Shell"];

            //以下是未使用的部份
            //J6M_Bone = model.Bones["J6_Motor_Interface-DC_Shell"];
            //HD32down_Bone = model.Bones["harmonic_drive_8200(CSG/CSF_32)-DC_Shell_down"];
            //HD32up_Bone = model.Bones["harmonic_drive_8200(CSG/CSF_32)-DC_Shell_up"];
            //HD25_Bone = model.Bones["harmonic_drive_8200(CSG/CSF_25)-DC_Shell"];
            //SHF17_Bone = model.Bones["SHF17-DC_Shell_out"];
            //J5R_Bone = model.Bones["J5_Left-DC_Shell"];
            //J5L_Bone = model.Bones["J5_Left-DC_Shell"];

            //關節要對應到正確的模型部件 修改 by 晉瑋 2013.01.05
            J1_Transform = J1_Bone.Transform; //底座
            J2_Transform = J2_Bone.Transform;
            J3_Transform = J3_Bone.Transform;
            J4_Transform = J4_Bone.Transform;
            J5_Transform = J5_Bone.Transform;
            J6_Transform = J6_Bone.Transform;
            CSF14_Transform = CSF14_Bone.Transform;

            // Allocate the transform matrix array.
            boneTransforms = new Matrix[model.Bones.Count];

            /*
            foreach (ModelMesh mesh in model.Meshes)
            {
                if (mesh.ParentBone == J1_Bone)
                {
                    BoneCount[0] += 1;
                    BBox[0] = BuildBoundingBox(mesh, J1_Transform);
                }
                else if (mesh.ParentBone == J2_Bone)
                {
                    BoneCount[1] += 1;
                    BBox[1] = BuildBoundingBox(mesh, J2_Transform);
                }
                else if (mesh.ParentBone == J3_Bone)
                {
                    BoneCount[2] += 1;
                    BBox[2] = BuildBoundingBox(mesh, J3_Transform);
                }
                else if (mesh.ParentBone == J4_Bone)
                {
                    BoneCount[3] += 1;
                    BBox[3] = BuildBoundingBox(mesh, J4_Transform);
                }
                else if (mesh.ParentBone == J5_Bone)
                {
                    BoneCount[4] += 1;
                    BBox[4] = BuildBoundingBox(mesh, J5_Transform);
                }
                else if (mesh.ParentBone == J6_Bone)
                {
                    BoneCount[5] += 1;
                    BBox[5] = BuildBoundingBox(mesh, J6_Transform);
                }

            }
            */
            // skc, 此處還在測試中
            // for (int i = 0; i < 6; i++)
            //    DebugShapeRenderer.AddBoundingBox(BBox[i], Color.Yellow);
            // DebugShapeRenderer.AddBoundingFrustum(frustum, Color.Green);
        }
        /// <summary>
        /// Draws the control.
        /// </summary>
        protected override void Draw()
        {
            // Clear to the default control background color.

            //Color backColor = new Color(BackColor.R, BackColor.G, BackColor.B);
            //GraphicsDevice.Clear(backColor);

            //test
            GraphicsDevice.Clear(Color.DarkGray);

            if (model != null)
            {
                // Compute camera matrices.
                // float rotation = (float)timer.Elapsed.TotalSeconds;
                modelCenter = Vector3.UnitY * modelRadius * 0.5f;
                Vector3 eyePosition = modelCenter;

                //x方向位移
                // eyePosition.X += modelRadius * 1f;
                eyePosition.X = modelCenter.X + modelRadius * 1.72f * (float)Math.Cos(CameraRotateValue) * (float)Math.Cos(CameraTiltValue);
                // eyePosition.Z += modelRadius * 1.4f;  // 調整 by 晉瑋 2012.12.17
                eyePosition.Z = modelCenter.Z + modelRadius * 1.72f * (float)Math.Sin(CameraRotateValue) * (float)Math.Cos(CameraTiltValue);
                // eyePosition.Y += modelRadius * 1.0f; // 調整 by 晉瑋 2012.12.17
                eyePosition.Y = modelCenter.Y + modelRadius * 2f * (float)Math.Sin(CameraTiltValue);

                //eyePosition.Z += modelRadius * 0.7f; // (原始值)
                //eyePosition.Y += modelRadius * 0.45f;  // (原始值)

                // modelCenter = Vector3.UnitY * modelRadius * 0.5f; // 調整 by 晉瑋 2012.12.17
                //modelCenter = Vector3.UnitY * modelRadius * 0.3f; // test (原始值)

                float aspectRatio = GraphicsDevice.Viewport.AspectRatio;

                float nearClip = modelRadius / 100;
                float farClip = modelRadius * 100;

                //模型座標軸旋轉
                world = Matrix.CreateRotationX((float)-Math.PI / 2.0f) * Matrix.CreateRotationY((float)Math.PI);
                view = Matrix.CreateLookAt(eyePosition, modelCenter, Vector3.Up);
                //正交投影
                //Matrix projection = Matrix.CreateOrthographic(GraphicsDevice.Viewport.Width*7,GraphicsDevice.Viewport.Height*7,nearClip,farClip);
                //透視投影
                projection = Matrix.CreatePerspectiveFieldOfView(1, aspectRatio, nearClip, farClip);
                frustum = new BoundingFrustum(view * projection);

                /*畫模型*/
                DrawRobot(world, view, projection);

                /*BasicEffect*/
                BasicEffect Beffect = new BasicEffect(GraphicsDevice);
                Beffect.VertexColorEnabled = true;
                Beffect.World = world;
                Beffect.Projection = projection;
                Beffect.View = view;
                Beffect.CurrentTechnique.Passes[0].Apply();

                /*畫世界座標軸*/
                //2013.05.03修改 : WireFrame模式下才要畫世界座標
                if (isWireMode)
                    GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, worldAxes, 0, worldAxes.Length / 2);

                /*畫關節控制圈*/
                //第一參數改為Index,用來決定畫哪一個ModelBone
                if (Circle_BoneIndexValue != 0)
                    DrawCircle(Circle_BoneIndexValue, world);

                /*畫Local座標軸*/
                //第一個參數為Index,用來決定畫哪一個ModelBone
                //if (LocalAxes_BoneIndexValue != 0)
                //    DrawLocalAxes(LocalAxes_BoneIndexValue, world);

                //固定畫出末端的座標軸方向 (預設值為末端)
                DrawLocalAxes(7, world);

                // skc, 測試
                // DebugShapeRenderer.Draw( view, projection);

                /*畫場景*/
                DrawRoom(world, view, projection);

            }
        }
        public void DrawRobot(Matrix world, Matrix view, Matrix projection)
        {

            //是否要顯示WireFrame模式
            if (isWireMode)
                GraphicsDevice.RasterizerState = WireState;
            else
                GraphicsDevice.RasterizerState = SolidState;

            // Set the world matrix as the root transform of the model.
            model.Root.Transform = world;

            //各關節依據各自的角度旋轉
            J1_Bone.Transform = Matrix.CreateRotationY(0) * J1_Transform;                     //底座 
            J2_Bone.Transform = Matrix.CreateRotationY(J1RotationValue) * J2_Transform;
            J3_Bone.Transform = Matrix.CreateRotationZ(J2RotationValue) * J3_Transform;
            J4_Bone.Transform = Matrix.CreateRotationX(J3RotationValue) * J4_Transform;
            J5_Bone.Transform = Matrix.CreateRotationX(J4RotationValue) * J5_Transform;
            J6_Bone.Transform = Matrix.CreateRotationZ(J5RotationValue) * J6_Transform;
            CSF14_Bone.Transform = Matrix.CreateRotationX(-1*J6RotationValue) * CSF14_Transform; //末端

            // Look up combined bone matrices for the entire model.
            model.CopyAbsoluteBoneTransformsTo(boneTransforms);

            // Draw the model.
            foreach (ModelMesh mesh in model.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.World = boneTransforms[mesh.ParentBone.Index];
                    effect.View = view;
                    effect.Projection = projection;

                    /*2013.05.03新增 : WireFrame有透明度*/
                    effect.GraphicsDevice.BlendState = BlendState.NonPremultiplied;

                    effect.EnableDefaultLighting();
                    effect.DirectionalLight0.Enabled = true;
                    effect.SpecularColor = new Vector3(0.05f);
                    effect.DirectionalLight0.Direction = new Vector3(-0.7f, -0.7f, -0.7f);

                    /*2013.05.03新增 : WireFrame有透明度*/
                    if (isWireMode) 
                        effect.Alpha = 0.2f;
                    else
                        effect.Alpha = 1f;


                    //若發生碰撞，用環境光上紅色
                    if (isCollision)
                        effect.AmbientLightColor = new Vector3(1f, 0, 0);
                    else
                        effect.AmbientLightColor = new Vector3(0, 0, 0);
                }
                mesh.Draw();
            }

        }

        public void BoneProcess_room()
        {
            // Allocate the transform matrix array for Room
            boneTransforms_room = new Matrix[room.Bones.Count];
        }

        //畫場景
        public void DrawRoom(Matrix world, Matrix view, Matrix projection)
        {
            //場景固定為SolidState，不要變成WireFrame
            GraphicsDevice.RasterizerState = SolidState;

            // Set the world matrix as the root transform of the model.
            room.Root.Transform = world;

            room.CopyAbsoluteBoneTransformsTo(boneTransforms_room);

            // Draw the model.
            foreach (ModelMesh mesh in room.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    //位置與比例調整
                    effect.World = boneTransforms_room[mesh.ParentBone.Index] * roomModify;
                    effect.View = view;
                    effect.Projection = projection;

                    effect.EnableDefaultLighting();
                    effect.DirectionalLight0.Enabled = true;
                    effect.AmbientLightColor = new Vector3(0.1f, 0.1f, 0.1f); //2013.05.6修改: 顏色協調
                    effect.DirectionalLight0.Direction = new Vector3(-0.7f, -0.7f, -0.7f);

                }

                mesh.Draw();
            }
        }

        /// <summary>
        /// Whenever a new model is selected, we examine it to see how big
        /// it is and where it is centered. This lets us automatically zoom
        /// the display, so we can correctly handle models of any scale.
        /// </summary>
        void MeasureModel()
        {
            // Look up the absolute bone transforms for this model.
            boneTransforms = new Matrix[model.Bones.Count];

            model.CopyAbsoluteBoneTransformsTo(boneTransforms);

            // Compute an (approximate) model center position by
            // averaging the center of each mesh bounding sphere.
            modelCenter = Vector3.Zero;

            foreach (ModelMesh mesh in model.Meshes)
            {
                BoundingSphere meshBounds = mesh.BoundingSphere;
                Matrix transform = boneTransforms[mesh.ParentBone.Index];
                Vector3 meshCenter = Vector3.Transform(meshBounds.Center, transform);

                modelCenter += meshCenter;
            }

            modelCenter /= model.Meshes.Count;

            // Now we know the center point, we can compute the model radius
            // by examining the radius of each mesh bounding sphere.
            modelRadius = 0;

            foreach (ModelMesh mesh in model.Meshes)
            {
                BoundingSphere meshBounds = mesh.BoundingSphere;
                Matrix transform = boneTransforms[mesh.ParentBone.Index];
                Vector3 meshCenter = Vector3.Transform(meshBounds.Center, transform);

                float transformScale = transform.Forward.Length();

                float meshRadius = (meshCenter - modelCenter).Length() +
                                   (meshBounds.Radius * transformScale);

                modelRadius = Math.Max(modelRadius, meshRadius);
            }
        }

        //畫local座標軸 參數BoneIndex為要畫部位的索引
        void DrawLocalAxes(int BoneIndex, Matrix world)
        {
            //Local座標軸的長度

            //因為C#的array為傳址，每次都需要重新new一次localAxes再放進List才會有效果
            localAxes = new VertexPositionColor[6];

            /*Local座標軸的點*/
            //變換矩陣 = boneModify + boneTransforms + world
            //x軸 - 紅色
            localAxes[0] = new VertexPositionColor(Vector3.Transform(new Vector3(0), boneModify[BoneIndex] * boneTransforms[BoneIndex] * world), Color.Red);
            localAxes[1] = new VertexPositionColor(Vector3.Transform(new Vector3(localAxisLength, 0, 0), boneModify[BoneIndex] * boneTransforms[BoneIndex] * world), Color.Red);
            //y軸 - 綠色
            localAxes[2] = new VertexPositionColor(Vector3.Transform(new Vector3(0), boneModify[BoneIndex] * boneTransforms[BoneIndex] * world), Color.Green);
            localAxes[3] = new VertexPositionColor(Vector3.Transform(new Vector3(0, localAxisLength, 0), boneModify[BoneIndex] * boneTransforms[BoneIndex] * world), Color.Green);
            //z軸 - 藍色
            localAxes[4] = new VertexPositionColor(Vector3.Transform(new Vector3(0), boneModify[BoneIndex] * boneTransforms[BoneIndex] * world), Color.Blue);
            localAxes[5] = new VertexPositionColor(Vector3.Transform(new Vector3(0, 0, localAxisLength), boneModify[BoneIndex] * boneTransforms[BoneIndex] * world), Color.Blue);

            //畫座標軸
            GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, localAxes, 0, localAxes.Length / 2);

            //若要記錄軌跡
            if (isRecordPathValue)
            {
                if (recordTimer.ElapsedMilliseconds >= threshold)
                {
                    //把當下的座標軸位置存入path裡
                    path.Add(localAxes);
                    //計時器歸零
                    recordTimer.Restart();
                }
            }

            //畫出path裡面所有的座標軸
            foreach (VertexPositionColor[] axis in path)
            {
                GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, axis, 0, axis.Length / 2);
            }
        }

        public void ClearPath()
        {
            path.Clear();
        }

        //任兩點畫線段的函式
        void DrawLine(Vector3 point1, Vector3 point2)
        {
            VertexPositionColor[] vpcs = new VertexPositionColor[2];
            vpcs[0] = new VertexPositionColor(point1, Color.Red);
            vpcs[1] = new VertexPositionColor(point2, Color.Red);

            //畫線段
            GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, vpcs, 0, 1);
        }

        //畫關節控制圈 參數BoneIndex為要畫部位的索引
        void DrawCircle(int BoneIndex, Matrix world)
        {
            //先以手動調整各關節控制圓的半徑...
            float radius;

            if (BoneIndex == 7)      //CSF14
                radius = 110;
            else if (BoneIndex == 5) //J6
                radius = 110;
            else if (BoneIndex == 4) //J5
                radius = 110;
            else if (BoneIndex == 3) //J4
                radius = 150;
            else if (BoneIndex == 2) //J3
                radius = 150;
            else if (BoneIndex == 1) //J2
                radius = 220;
            else
                radius = 150;

            //預設讓圓由120個點畫成
            Vector3[] VertexPoints = new Vector3[120];

            //先在平面畫圓
            Vector3 center = new Vector3(radius, 0, 0);
            Matrix tranmatrix;
            for (int i = 0; i < 120; i++)
            {
                //圓圈中每一點的位置
                tranmatrix = new Matrix((float)Math.Cos(MathHelper.ToRadians(3)), (float)Math.Sin(MathHelper.ToRadians(3)), 0, 0,
                   -1 * (float)Math.Sin(MathHelper.ToRadians(3)), (float)Math.Cos(MathHelper.ToRadians(3)), 0, 0,
                   0, 0, 0, 0,
                   0, 0, 0, 0
                   );
                center = Vector3.Transform(center, tranmatrix);
                VertexPoints[i] = new Vector3(center.X, center.Y, 0);
            }

            //把整個圓移到Bone的位置 (變換矩陣 = boneModify + boneTransforms + world)
            for (int i = 0; i < VertexPoints.Length; i++)
            {
                VertexPoints[i] = Vector3.Transform(VertexPoints[i], boneModify[BoneIndex] * boneTransforms[BoneIndex] * world);
            }

            Vector3 screenPos = GraphicsDevice.Viewport.Project(Vector3.Zero, projection, view, boneModify[BoneIndex] * boneTransforms[BoneIndex] * world);
            circleCenter.X = screenPos.X;
            circleCenter.Y = screenPos.Y;

            //用線段來畫圓
            for (int i = 0; i < VertexPoints.Length; i++)
            {
                Vector3 point1;
                Vector3 point2;
                if (i == VertexPoints.Length - 1)
                {
                    point1 = VertexPoints[i];
                    point2 = VertexPoints[0];
                }
                else
                {
                    point1 = VertexPoints[i];
                    point2 = VertexPoints[i + 1];
                }
                DrawLine(point1, point2);
            }
        }

    }
}
